package com.whity.towerdefense.model;

import java.util.Collections;
import java.util.Comparator;
import java.util.LinkedList;
import java.util.List;
import java.util.Vector;
import java.util.concurrent.Semaphore;

import com.whity.towerdefense.Pair;

import android.graphics.Point;
import android.graphics.PointF;
import android.util.Log;


public class PathfindingThread extends Thread
{
	// =============================================================
	// Private attributs
	
	private class PathPair
	{
		public I_Pathfindable 		elt;
		public PointF				goal;
	}
	
	// Constants
	@SuppressWarnings("unused")
	private static final String 	TAG = "com.djm.test03.model";
	
	@SuppressWarnings("unused")
	private Map						m_Map;
	private LinkedList<PathPair>	m_Elts;
	private Semaphore				m_SemaphorePool;
	
	// =============================================================
	// Public functions :
	
	public PathfindingThread(Map p_Map)
	{
		m_Map = p_Map;
		m_Elts = new LinkedList<PathPair>();
		m_SemaphorePool = new Semaphore(0);
	}
	
	public void AddPathfindable(I_Pathfindable p_Elt, Point p_Goal)
	{
		PathPair pair = new PathPair();
		pair.elt = p_Elt;
		
		PointF goal = new PointF();
		goal.x = p_Goal.x+0.5f;
		goal.y = p_Goal.y+0.5f;
		pair.goal = goal;
		
		synchronized(m_Elts)
		{
			m_SemaphorePool.release();
			m_Elts.add(pair);
		}
	}

	public void run()
	{
		while(true)
		{
			try
			{
				m_SemaphorePool.acquire();
			
				PathPair pair;
				synchronized(m_Elts)
				{
					pair = m_Elts.poll();
				}
				pair.elt.PathfinderCallback(CalculatePath(pair.elt, pair.goal));

			}
			catch (InterruptedException e)
			{
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
	
	private Path CalculatePath(I_Pathfindable p_Elt, PointF p_Goal)
	{
		Path res = new Path();
		
		Vector<Cell> cells = m_Map.GetCells();
		int mapWidth = m_Map.GetWidthInCells();
		int startIndex = p_Elt.GetCellPosition().y * mapWidth + p_Elt.GetCellPosition().x;
		int goalIndex = (int) (Math.floor(p_Goal.x) + Math.floor(p_Goal.y)*mapWidth);
		Cell goal = cells.get(goalIndex);
		res.PushEnd(p_Goal);

		
		float[] g_score = new float[cells.size()];
		g_score[0] = 0.0f;
		float[] h_score = new float[cells.size()];
		h_score[startIndex] = heuristique(cells.get(startIndex), goal);
		
		Vector<Cell> closedSet = new Vector<Cell>(100);
		List<Pair<Float, Cell>> openSet = new Vector<Pair<Float, Cell>>(100);
		openSet.add(new Pair<Float, Cell>(h_score[startIndex]+g_score[startIndex], cells.get(startIndex)));
		int[] cameFrom = new int[cells.size()];
		
		Comparator<Pair<Float, Cell>> comparator = new Comparator<Pair<Float, Cell>>()
			{
				public int compare(Pair<Float, Cell> lhs, Pair<Float, Cell> rhs)
				{
					return lhs.first<rhs.first?-1:1;
				}
				
			};
		while(!openSet.isEmpty())
		{
			Collections.sort(openSet, comparator);
			Cell x = openSet.remove(0).second;
			int xIndex = getIndex(x);
			closedSet.add(x);
			
			if(xIndex == goalIndex)
			{
				int fatherIndex = goalIndex;
				
				do
				{
					fatherIndex = cameFrom[fatherIndex];
					x = cells.get(fatherIndex);
					PointF addPoint = new PointF(x.m_Position.x + 0.5f, x.m_Position.y + 0.5f);
					res.PushFront(addPoint);
				}
				while(fatherIndex != startIndex);
				
				Log.d(TAG, "Path founded : " + res);
			}
			
			List<Cell> neighbourgs = x.GetNeighbour();
			for(Cell neighbour : neighbourgs)
			{
				int neighbourgIndex = getIndex(neighbour);
				if(closedSet.contains(neighbour))
				{
					continue;
				}
				float temp_g_score = g_score[xIndex]+neighbour.CostToCross();
				
				Pair<Float, Cell> found = null;
				for(Pair<Float, Cell> pair : openSet)
				{
					if(pair.second == neighbour)
					{
						found = pair;
						break;
					}
				}
				
				if(found == null)
				{
					g_score[neighbourgIndex] = temp_g_score;
					h_score[neighbourgIndex] = heuristique(neighbour, goal);
					openSet.add(new Pair<Float, Cell>(g_score[neighbourgIndex] + h_score[neighbourgIndex], neighbour));
					cameFrom[neighbourgIndex] = xIndex;
					
					
				}
				else if(temp_g_score < g_score[neighbourgIndex])
				{
					cameFrom[neighbourgIndex] = xIndex;
					g_score[neighbourgIndex] = temp_g_score;
					found.first = g_score[neighbourgIndex] + h_score[neighbourgIndex];
				}
			}
		}
		return res;
	}
	
	private float heuristique(Cell p_Start, Cell p_End)
	{
		return (p_End.m_Position.x-p_Start.m_Position.x)*(p_End.m_Position.x-p_Start.m_Position.y)+(p_End.m_Position.y-p_Start.m_Position.y)*(p_End.m_Position.y-p_Start.m_Position.y);
	}
	
	private int getIndex(Cell p_Cell)
	{
		return p_Cell.m_Position.x + p_Cell.m_Position.y * m_Map.GetWidthInCells();
	}
	
	// =============================================================
}
